#include "app_motor_pwm.h"
#include "asm/mcpwm.h"

// 电机控制相关参数
static u32 motor_duty = 10000;    // 当前占空比（0-10000）注意：0=最大功率，10000=最小功率
static u8 motor_running = 0;   // 电机运行状态标志


static u32 pwm_to_voltage_mv(u32 pwm) {
    // 基于完整实测数据表，选择16个关键点保证精度
    // 数据来源：实际测量，电压单位mV，PWM范围0-6700
    const struct { u32 p; u32 v; } pts[] = {
        {0,12340}, {100,12220}, {500,12290}, {1000,11100},
        {1500,10400}, {2000,9870}, {2500,9180}, {3000,8580},
        {3500,8080}, {4000,7410}, {4500,6870}, {5000,6160},
        {5500,5520}, {6000,4880}, {6300,4560}, {6700,4040}
    };

    // 线性插值算法：V = V1 + (V2-V1) × (PWM-PWM1) / (PWM2-PWM1)
    for (int i = 0; i < 15; i++) {
        if (pwm >= pts[i].p && pwm <= pts[i+1].p) {

            u32 p1=pts[i].p, p2=pts[i+1].p, v1=pts[i].v, v2=pts[i+1].v;
            // 防止整数溢出，使用更安全的计算方式
            u32 delta_p = p2 - p1;
            u32 delta_v = (v2 > v1) ? (v2 - v1) : (v1 - v2);
            u32 offset_p = pwm - p1;

            u32 result;
            if (v2 > v1) {
                result = v1 + (delta_v * offset_p) / delta_p;
            } else {
                result = v1 - (delta_v * offset_p) / delta_p;
            }

            return result;
        }
    } 
    return 6160; // 默认值
}

/**
 * @brief 获取电机电压(毫伏)
 * @return 电压值(毫伏)
 */
u32 app_motor_get_voltage_mv(void)
{
    return pwm_to_voltage_mv(motor_duty);
}

/**
 * @brief 获取电机电压(伏特)
 * @return 电压值(伏特)
 */
float app_motor_get_voltage_v(void)
{
    u32 voltage_mv = app_motor_get_voltage_mv();

    // 初始化变量避免垃圾值
    u32 voltage_v_int = 0;
    u32 voltage_v_decimal = 0;
    float voltage_v = 0.0f;

    // 计算整数部分和小数部分
    voltage_v_int = voltage_mv / 1000;  // 整数部分
    voltage_v_decimal = (voltage_mv % 1000) / 100;  // 小数部分(一位)
    voltage_v = voltage_v_int + voltage_v_decimal * 0.1f;

    printf("[MOTOR DEBUG] duty=%d, voltage_mv=%d, voltage_v_int=%d, voltage_v_decimal=%d\n",
           motor_duty, voltage_mv, voltage_v_int, voltage_v_decimal);
    return voltage_v;
}

/**
 * @brief 初始化电机PWM控制
 * 配置PWM参数并初始化硬件
 *
 * 使用通道0，PA4引脚作为PWM输出
 * 频率：20kHz，适合电机控制
 * 初始占空比：50%（5000/10000）
 */

 static struct pwm_platform_data pwm_pa4_cfg = {
	.pwm_aligned_mode = pwm_edge_aligned,   // 边沿对齐
	.pwm_ch_num       = pwm_ch0,            // 通道0
	.frequency        = 20000,              // 20k Hz
	.duty             = 10000,               // 50.00%
	.h_pin            = IO_PORTA_04,        // PA4 引脚
	.l_pin            = -1,                 // 无低端
	.complementary_en = 0,                  // 关闭互补
};

void app_motor_init(void)
{
    // 初始化PWM硬件配置
    mcpwm_init(&pwm_pa4_cfg);

    // 设置初始占空比
    mcpwm_set_duty(pwm_ch0, motor_duty);

    // 设置初始状态为运行
    motor_running = 1;

    printf("[MOTOR] PWM INIT\n");
    printf("[MOTOR] Initial duty: %d\n", motor_duty);
}

/**
 * @brief 启动电机
 * 启动PWM输出，使用当前占空比开始运行
 * 注意：如果当前占空比为0，自动设置为50%运行
 */
void app_motor_start(void)
{
    if (motor_running) {
        return;  // 电机已经在运行中，避免重复启动
    }
    if (motor_duty == 10000) {
        motor_duty = 5000;  // 如果已关闭，设为50%运行
        printf("[MOTOR] Auto set to 50%% duty\n");
    }

    mcpwm_set_duty(pwm_ch0, motor_duty);
    motor_running = 1;

    printf("[MOTOR] Motor started\n");
    printf("[MOTOR] Current duty: %d\n", motor_duty);
}

/**
 * @brief 停止电机
 * 通过设置占空比接近10000来停止电机
 *
 */
void app_motor_stop(void)
{
    mcpwm_set_duty(pwm_ch0, 10000);  // 设置高占空比，低功率运行
    motor_running = 0;

    printf("[MOTOR] Motor stopped\n");
    printf("[MOTOR] PWM set to low power mode\n");
}

/**
 * @brief 设置电机占空比
 * @param duty 占空比值（0-10000）
 * 重要说明：
 * - duty=0：最大功率运行，约12.3V
 * - duty=10000：最小功率运行，约4V，接近停止
 * - 占空比与输出电压成反比关系
 * - 占空比越低，输出电压越高，电机功率越大
 */
void app_motor_set_duty(u32 duty)
{
    if (duty > 10000) {
        duty = 10000;  // 限制最大值
    }

    motor_duty = duty;
    mcpwm_set_duty(pwm_ch0, motor_duty);

    // 更新运行状态，占空比接近10000时视为已停止状态
    motor_running = (duty < 9000) ? 1 : 0;

    printf("[MOTOR] 占空比设置为: %d\n", motor_duty);
}

/**
 * @brief 获取当前占空比
 * @return 当前占空比值（0-10000）
 */
u32 app_motor_get_duty(void)
{
    return motor_duty;
}

/**
 * @brief 获取电机运行状态
 * @return 1=正在运行，0=停止
 */
u8 app_motor_get_status(void)
{
    return motor_running;
}

/**
 * @brief 电机主循环处理
 * 定时调用，用于处理电机相关的状态监控
 */
void app_motor_main_handler(void)
{
    app_motor_adc_read();
}

/**
 * @brief PWM电压读取（简化版）
 */
void app_motor_adc_read(void) {


}
